% 同心鼓协作策略优化模型（只求颠一次球）
clear all; close all; clc;

%% 物理参数设置
m_ball = 0.270;        % 排球质量(kg)
m_drum = 3.6;          % 鼓的质量(kg)
g = 9.8;               % 重力加速度(m/s^2)
r_drum = 0.2;          % 鼓面半径(m)
h_drum = 0.22;         % 鼓身高度(m)
rope_length = 1.7;     % 绳子长度(m)
initial_height = 0.4;  % 球初始高度(m)
required_height = 0.4; % 要求颠起高度(m)
num_people = 8;        % 人数
min_distance = 0.6;    % 最小间距(m)
initial_drum_drop = 0.11; % 鼓的初始下降位移(m)

%% 几何布局计算
% 计算人员位置（均匀分布在圆周上）
angle_step = 2*pi/num_people;
person_positions = zeros(num_people, 2);
for i = 1:num_people
    angle = (i-1) * angle_step;
    % 人员位置半径计算，确保间距≥60cm
    person_radius = min_distance / (2 * sin(angle_step/2));
    person_positions(i,:) = [person_radius * cos(angle), person_radius * sin(angle)];
end

% 计算绳子固定点位置（鼓面边缘均匀分布）
rope_anchor_points = zeros(num_people, 3);
for i = 1:num_people
    angle = (i-1) * angle_step;
    rope_anchor_points(i,:) = [r_drum * cos(angle), r_drum * sin(angle), 0];
end

%% 时间离散化参数
dt = 0.001;            % 时间步长(s)
total_time = 2;        % 总仿真时间(s)，只求颠一次球，时间可缩短
time_steps = total_time / dt;

%% 初始条件
% 球的初始状态
ball_position = [0, 0, initial_height];
ball_velocity = [0, 0, 0];

% 鼓的初始状态（考虑初始下降位移）
drum_position = [0, 0, -initial_drum_drop];
drum_velocity = [0, 0, 0];

% 记录最优解
best_strategy = zeros(num_people, 1);
max_height = 0;
min_total_force = inf;
best_time_offset = 0;
best_force_magnitude = 0;
best_force_duration = 0;

%% 优化参数设置（缩小搜索空间，专注于第一次碰撞）
% 发力时机范围（碰撞前0.3秒到碰撞后0.1秒）
time_offset_range = linspace(-0.3, 0.1, 20);
% 发力持续时间范围
force_duration_range = linspace(0.05, 0.2, 10);
% 力的大小范围
force_magnitude_range = linspace(20, 150, 30);

%% 主优化循环
fprintf('开始优化同心鼓协作策略（只求颠一次球）...\n');

% 标记是否找到有效解
solution_found = false;

% 遍历不同的发力持续时间
for d_idx = 1:length(force_duration_range)
    force_duration = force_duration_range(d_idx);
    
    % 遍历不同的发力时机
    for t_idx = 1:length(time_offset_range)
        time_offset = time_offset_range(t_idx);
        
        % 遍历不同的发力大小
        for f_idx = 1:length(force_magnitude_range)
            force_magnitude = force_magnitude_range(f_idx);
            
            % 初始化存储数组
            ball_pos_history = zeros(time_steps, 3);
            ball_vel_history = zeros(time_steps, 3);
            drum_pos_history = zeros(time_steps, 3);
            drum_vel_history = zeros(time_steps, 3);
            force_history = zeros(time_steps, num_people);
            
            % 重置状态
            current_ball_pos = ball_position;
            current_ball_vel = ball_velocity;
            current_drum_pos = drum_position;
            current_drum_vel = drum_velocity;
            
            % 记录最大高度和是否发生碰撞
            current_max_height = 0;
            collision_occurred = false;
            collision_step = 0;
            
            % 仿真循环
            for step = 1:time_steps
                t = (step-1) * dt;
                
                % 计算重力
                ball_gravity_force = [0, 0, -m_ball * g];
                drum_gravity_force = [0, 0, -m_drum * g];
                
                % 计算当前力（假设所有人同时发力）
                current_force = zeros(num_people, 3);
                if t >= time_offset && t <= time_offset + force_duration
                    % 计算每个人的发力方向（沿绳子方向）
                    for i = 1:num_people
                        % 绳子方向向量（从人位置到鼓当前位置）
                        rope_direction = current_drum_pos(1:2) - person_positions(i,:);
                        rope_direction = [rope_direction, current_drum_pos(3)];
                        rope_direction = rope_direction / norm(rope_direction);
                        
                        % 施加力（方向沿绳子向上）
                        current_force(i,:) = force_magnitude * rope_direction;
                        
                        % 记录力的大小
                        force_history(step, i) = force_magnitude;
                    end
                end
                
                % 合力
                total_force = sum(current_force, 1);
                
                % 更新鼓的运动状态
                drum_acceleration = (drum_gravity_force + total_force) / m_drum;
                current_drum_vel = current_drum_vel + drum_acceleration * dt;
                current_drum_pos = current_drum_pos + current_drum_vel * dt;
                
                % 更新球的运动状态
                ball_acceleration = ball_gravity_force / m_ball;
                current_ball_vel = current_ball_vel + ball_acceleration * dt;
                current_ball_pos = current_ball_pos + current_ball_vel * dt;
                
                % 检测第一次碰撞（球接触鼓面）
                if ~collision_occurred && current_ball_pos(3) <= current_drum_pos(3) && current_ball_vel(3) < 0
                    collision_occurred = true;
                    collision_step = step;
                    
                    % 弹性碰撞处理（动量和能量守恒）
                    v_ball_before = current_ball_vel(3);
                    v_drum_before = current_drum_vel(3);
                    
                    % 弹性碰撞后的速度
                    v_ball_after = ((m_ball - m_drum) * v_ball_before + 2 * m_drum * v_drum_before) / (m_ball + m_drum);
                    v_drum_after = ((m_drum - m_ball) * v_drum_before + 2 * m_ball * v_ball_before) / (m_ball + m_drum);
                    
                    % 更新碰撞后的速度
                    current_ball_vel(3) = v_ball_after;
                    current_drum_vel(3) = v_drum_after;
                    
                    % 调整位置防止穿透
                    current_ball_pos(3) = current_drum_pos(3);
                end
                
                % 记录位置和速度
                ball_pos_history(step,:) = current_ball_pos;
                ball_vel_history(step,:) = current_ball_vel;
                drum_pos_history(step,:) = current_drum_pos;
                drum_vel_history(step,:) = current_drum_vel;
                
                % 更新最大高度（只考虑碰撞后的高度）
                if collision_occurred && current_ball_pos(3) > current_max_height
                    current_max_height = current_ball_pos(3);
                end
                
                % 如果球达到足够高度并开始下降，记录结果
                if collision_occurred && current_ball_vel(3) < 0 && current_ball_pos(3) > current_drum_pos(3) + required_height
                    % 计算总发力（只考虑碰撞前的发力）
                    if step > collision_step
                        total_force_applied = sum(sum(force_history(1:collision_step,:)));
                    else
                        total_force_applied = sum(sum(force_history));
                    end
                    
                    % 更新最优解
                    if current_max_height >= required_height + current_drum_pos(3) && total_force_applied < min_total_force
                        solution_found = true;
                        min_total_force = total_force_applied;
                        best_strategy = force_magnitude * ones(num_people, 1);
                        max_height = current_max_height - current_drum_pos(3);
                        best_time_offset = time_offset;
                        best_force_magnitude = force_magnitude;
                        best_force_duration = force_duration;
                    end
                    
                    % 提前结束仿真
                    break;
                end
            end
        end
    end
end

%% 结果输出
fprintf('优化完成!\n');
if solution_found
    fprintf('最佳协作策略（只求颠一次球）:\n');
    fprintf('  发力时机: 碰撞前 %.3f 秒\n', -best_time_offset);
    fprintf('  发力持续时间: %.3f 秒\n', best_force_duration);
    fprintf('  发力大小: %.2f N\n', best_force_magnitude);
    fprintf('  每人平均最小拉力: %.2f N\n', best_force_magnitude);
    fprintf('  颠球高度: %.3f m (相对于鼓面)\n', max_height);
    
    % 计算并显示鼓的速度对颠球效果的影响
    ball_velocity_before_collision = sqrt(2 * g * initial_height);
    drum_velocity_at_collision = best_force_magnitude * best_force_duration / m_drum;
    expected_bounce_height = ((m_ball - m_drum) * ball_velocity_before_collision + 2 * m_drum * drum_velocity_at_collision)^2 / (2 * g * (m_ball + m_drum)^2);
    
    fprintf('\n理论分析:\n');
    fprintf('  球碰撞前速度: %.2f m/s\n', ball_velocity_before_collision);
    fprintf('  鼓碰撞时速度: %.2f m/s\n', drum_velocity_at_collision);
    fprintf('  理论计算颠球高度: %.3f m\n', expected_bounce_height);
else
    fprintf('未找到满足条件的解，请进一步调整搜索参数范围。\n');
    fprintf('提示: 可以尝试更大的力范围或更长的发力持续时间。\n');
end

%% 可视化
if solution_found
    figure;
    plot(1:time_steps, ball_pos_history(:,3), 'b-', 'LineWidth', 1.5);
    hold on;
    plot(1:time_steps, drum_pos_history(:,3), 'r-', 'LineWidth', 1.5);
    xlabel('时间步');
    ylabel('高度 (m)');
    title('球和鼓的高度变化');
    legend('球', '鼓');
    grid on;
    
    % 绘制力的施加情况
    figure;
    plot(1:time_steps, force_history(:,1), 'b-', 'LineWidth', 1.5);
    xlabel('时间步');
    ylabel('拉力大小 (N)');
    title('团队发力情况');
    grid on;
end